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ABSTRACT 



The Kalman Filter is implemented to process range data output from 
the Cubic Model 40 Autotape system, a surface position locating system 
currently employed on the underwater tracking ranges at Dabob Bay and 
Nanoose. Results are presented for different measurement noise and 
forcing function noise statistics. 
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I. INTRODUCTION 



The Cubic CM-40 Autotape is a microwave distance measuring system 
used (by the U.S. Navy at its acoustic underwater tracking ranges at 
Dabob Bay and Nanoose) to provide reference position information for 
units on the surface and in the air above the range. This portable 
system consists basically of an interrogator which is operated aboard 
the unit to be tracked, two responders operated at two different shore 
sites and the associated antenna/RF assemblies. Required support systems 
include a data display and recording setup and an ADP facility for off- 
line processing of the Autotape data. Figure 1 shows the Autotape system 
components and Figure 2 shows a typical application geometry. 

Historically, the Autotape has been used in such applications as 
tracking hydrophone array survey, buoy and hydrophone array planting and 
as a reference position indicator for calibrating other position-finding 
devices against. Generally, the Autotape has been used where an extremely 
high degree of accuracy is not required. 

In operation, the system will provide for the display and recording 
of two ranges simultaneously, once per second, the ranges being those 
between the interrogator and each of the responders. The ranges are 
computed from the phase delay between the output of the modulation signal 
generator and a signal which has traveled from the interrogator to a 
responder and back. Ranging accuracy is stated by the manufacturer to 
be ji 0.5 meter + 10 ppm x range. Ranging frequencies of 1500 KHZ, 150 
KHZ and 165 KHZ modulate a 3000 MHZ carrier, yielding a maximum unambiguous 
range of 10,000 meters with a resolution of 0.1 meter. However, independent 



6 






•It 
. jnf 
ti 




•i 

•b 



•1 



t 





INTERROGATOR 



INTERROGATOR 
ANTENNA / RF 
ASSEMBLY 



RESPONDER 

RESPONDER ANTENNA 
/RF ASSEMBLY 



FIGURE 1: Cubic Model 40 Autotape System 
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FIGURE 2: Typical Autotape Application Geometry 
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testing by the U.S. Navy [Reference 1] has shown that system accuracy may 
not be quite as good as stated by the manufacturer. 

The accuracy of the Autotape system is principally dependent upon 
range errors, the geometry of the system and the method of data reduction. 
These factors are, in turn, affected by propagation velocity, system 
stability, range dependency, land survey accuracy, system geometry, 
slope reduction and data smoothing. A final anomaly which, depending 
upon the application, can substantially degrade the quality of the data- 
stream out is the orientation, over time, of the interrogator antenna in 
the vertical dimension. The interrogator antenna has only a 10 degree 
vertical beam width. Thus, if the system is being used on a platform 
such as a moderately maneuvering helicopter or a ship rolling substan- 
tially in the seaway, the system tends to frequently lose track, resulting 
in fairly long streams of useless data. 

Present data reduction techniques employed when the system is used 
on either of the ranges (Dabob or Nanoose) employ two overall iterations. 
The first, or initial processing, administers the following three correc- 
tions to the raw range data: 

1. Range Calibration Correction: This is a fixed value (meters) added 

to or subtracted from each range. 

2. Propagation Velocity Correction: This is a variable correction due 

to the atmospheric index of refraction at the particular time and 
place of the exercise. 

3. Slope Reduction Correction: This reduces both range measurements 

(which are actually slant ranges because the interrogator and the 
responders are not normally located at the exact same elevation) 
to a common horizontal plane at sea level. 

Subsequent processing of the data includes conversion of the corrected 
ranges to a rectangular x-y range coordinate system and a moving average 
smoothing technique which employes curve fitting algorithms (linear. 
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parabolic or logarithmic) to reduce the data to its final form. Not 
uncommonly, as a result of the total reduction effort, the net remainder 
is an inadequate data package (in terms of quantity) for proper final 
evaluation. 

Figure 3 is a rectangular plot of the raw ranges recorded during a 
recent array survey. The purpose of this project has been to design a 
filter, a Kalman filter, which would provide more accurate range data, 
as well as one that would track through the periods of "lost track" 
ranging, thereby providing a significantly larger final volume of data 
for evaluation. This paper presents the basic theory necessary and 
includes the final version of the filter. 
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FIGURE 3: Rectangular Plot of Raw Range Data 
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II. THE FILTER THEORY AND DESIGN 



A. THE SYSTEM DYNAMIC MODEL 

A common application for the Autotape system is its use as a reference 
position locator on the surface unit conducting an acoustic hydrophone 
array (range) survey. The usual exercise plan will call for a service 
unit, carrying the interrogator and equipped with an acoustic pinger 
mounted on the underwater hull, to transit three concentric circular 
tracks, centered above the array, with track radii ranging from 100 to 
1,000 meters, at speeds of up to eight knots. The direction of rotation 
for the outer track will normally be opposite to that of the middle 
circle. While the service unit is being tracked via Autotape, it is also 
being tracked by the acoustic array. By comparing the acoustic position 
data with that from the Autotape, a digital computer is able to compute 
actual position and attitude of the array. 

The desired estimates will be those of position and velocity, Rl, R2, 

Rl, R2. It is proper at' this point to define a number of terms and to 

summarize some pertinent results of observer theory. First, we may define 

a fourth order state vector: 

Rl 
R2 
Rl 
R^ 

Recall that a linear system can be described in the continuous time 
domain as: 

^ (t) = A X (t) + D w (t) 
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where : 



_x(t) is the n-element column vector of the states 

A and are nxn and nxp matrices describing system dynamics 

w(t) is a q-element vector of random noise inputs to the 
system 

The system measurements may be expressed as: 

^(t) = _x (t) + ^ (t) 

where: _z(t) is the q-element vector of system measurements 

is the qxn weighting matrix for the measurements 
^(t) is the q-element vector of random measurement noise 

The corresponding linear discrete model may be written as: 
x(k + 1) = ^)^(k) + r w(k) 

with no deterministic inputs to the system. 

Also, ^(k) = IH )^(k) + v(k) 



For the system under consideration, it can be shown that the state 
transition matrix 



0 



1 

0 

0 

0 



0 1 0 
1 0 1 
0 1 0 
0 0 1 



0 

0 % 

and £=10 

0 1 

for a sampling interval T of 1 second. A block diagram of the system is 
shown in Figure 4. 
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FIGURE 4: Block Diagram of Discrete Linear Estimator 




The following assumptions will be made regarding the noise processes 



and the initial state, )^(o) of the plant [Ref. 2]: 



The measurement noise has zero mean, is uncorrelated, and 

E [ y^(k) = ^(k) where % is the kronecker delta 

The forcing noise has zero mean, is uncorrelated, and 
E [ w(k) w^(j)] = 2.(k) 

The forcing noise and measurement noise are uncorrelated. 

The initial state is a random variable with known mean and covariance, 
and 

-s T. 



E [ Jx(o) - [^(o) - x^^ ^ ^ 



The measurement noise and initial state are uncorrelated. 

The forcing noise and initial state are uncorrelated. 

The Kalman Filter equations and their derivation are well known 
[Ref. 2], [Ref. 3]: 

6(k) = P(k/k-l) H^(k) [H(k) P(k/k-l) H^(k) + R(k)]~^ (1) 

P(k/k-l) = 0 P(k-l/k-l) i*" + Q (2) 

^ 

P(k/k) = n - 6(k) H(k)l P(k/k-l) (3) 

^ x(k/k) = x(k/k-l) + G(k) [z(k) - H(k) x(k/k-l)] (4) 

x(k/k-l) = 0(k/k-l) x(k-l/k-l) + r(k/k-l) w(k-l) (5) 



Where the notation (k/k-1) interprets as the value of the parameter of 
note at time k given measurements at times up to and including time k-1. 
(k/k) and (k-l/k-1) have similar interpretations. The x^ denotes the 
estimate of x. 
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^(k) represents the filter gain at time k. £ represents the covariance 
of estimation error; 



£ (k/k) = E [^(k/k) e^(k/k)] = E 



e^(k/k) 
62 (k/k) 






e^(k/k) 



[e^(k/k) e 2 (k/k)— e^(k/k)] 



/n 



= E < 



e^(k/k) 



n\ 






e^(k/k) e 2 (k/k) ™ e^(k/k) e^(k/k) 



e 2 (k/k) e^(k/k) e^{k/k) 



— e^{k/k) e^(k/k) 






e„(k/k) ej(k/k) e„(k/k) e^Ck/k) — e‘(k/k) 






where ^(k/k) = ^(k/k) - )((k). A complete standard block diagram for the 
filter and an information flow diagram are included as Figures 5 and 6 
as slightly different viewpoints from which the system may be viewed and 
understood. Figure 7 shows a timing diagram of the various quantities 
contained in the filter equations. 
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FIGURE 5: Kalman Filter Block Diagram 



17 



W',' 



» 

4 

I 

* 

4 

( 



I 

I 

j 

I 

» 

t 

I 



» 

I 

i 

I 

I 

r 

I 

i 



II 

‘V.- 

ii 



I 

I 





FIGURE 6: Simplified Information Flow Diagram of a Discrete Kalman Filter 
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FIGURE 7: Timing Diagram of Filter Equation Quantities 
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B. THE PROCESSOR 

Appendix A is a flowchart of the Kalman filter program utilized. 
Initially, the matrices describing the physical system, the noise 
statistics and other program parameters are read into storage and printed 
out. The discrete state-transition matrix. Phi, is computed and printed 
out and the gajn schedule is computed and printed out. It is seen that 
the elements of the gain matrix reach a steady state, and, for example, 
with both the £ and ^matrices being identity matrices, the gain reaches 
steady state between k=5 and k=10. Therefore, in the main iteration loop, 
the filter will essentially be a constant gain filter for k>10. 

Next, the main iteration loop commences. The initial measurements 
are read and xl(0/-l) and x2(0/-l) are initialized to these values. 
x3(0/-l) and x4(0/-l), representing the rates, are set to the mean 
constant value (in the respective directions) of 4.0 meters per second. 

The Autotape output is a 5 significant figure output, modulo 10,000, 
reading to 0.1 meter. Inherent in the output is a major degree of jitter 
in the two most significant digits, which would significantly distort 
the covariance of measurement noise. Therefore, as an option, measure- 
ments could be gated, and the gain automatically set to zero in those 
cases where the residue falls outside of a maximum reasonable bound. 

Commencing with k=0, and utilizing the known values for ^(0/-l) and 
£(0/-l), the Kalman filter equations are solved iteratively in the 
following manner [see page 15, equations (l)-(5)]: 

(1), (3), (4), 

Increment k to k=l 
(5), (2), (1), (3), (4), 

Increment k to k=2 

(5), (2), (1), (3), (4), 

etc. 
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Also computed on each iteration are the error residues: 

^ = z - x(k/k-l) 
and the one-step prediction errors: 

ERR = x(l</k) - x(k/k-l) 

Finally, the computations are tabulated and plots are produced. 

C. NOISE AND ERROR CONSIDERATIONS 

Reference 1 documents an Autotape evaluation which was conducted in 
1971. The error geometry is shown in Figure 8. Graphically, position 
is determined by locating the crossing point of the two range arcs, in 
conjunction with a knowledge of the baseline formed by the two responders. 
Since each range has an associated standard deviation (error), the point 
can actually be enclosed in a parallelogram which defines the probable 
position within one standard deviation of the ranges. The shape of the 
parallelogram will vary with the position of the crossing point relative 
to the baseline, as indicated in Figure 8. It can be shown that the 
maximum probable error (MPE) will be minimized where the range arcs are 
orthogonal. Figure 9 diagrams error contours which are actually the 
locii of constant MPE for two particular responder sites on the Nanoose 
Range. Table 1 summarizes pertinent results of the study. 
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FIGURE 8: Error and Geometry. At interrogator position 1, the range arcs are nearly orthogonal, and MPE is 
minimized. At interrogator position 2, the range arcs are not orthogonal, and MPE is greater. 
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FIGURE 9: Error Contours (Arcs represent maximum probable positional error in feet. End points of arcs 

are responder locations.) 
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TABLE 1 


Average Range Errors (feet) 






R- 


■1 


R- 


■2 


Survey 


No. Points 


Error 

Average 


Standard 

Deviation 


Error 

Average 


Standard 

Deviation 


Array 04 


30 


- 0.5 


2.8 


- 0.1 


2.8 


Array 07 


49 


- 1.3 


2.3 


- 0.4 


2.4 


Array 08 


10 


- 0.8 


4.4 


1.6 


2.8 


Array 09 


25 


3.8 


2.6 


0. 


2.2 


Average 




0.3 


3.0 


- 0.5 


2.6 



24 






\fTlA 

XftTiA 

'C«TiA 



For the purpose of modeling the covariance of excitation noise, it 
was assumed that the service unit transited an 800 meter circle at an 
average speed of eight knots. Then: 



v2. 


(8 kts) 1 


fl830 meteifF 
[ n. mile ) 


2 


R 


3600 


sec 

Hr 





800 meters 



= .0207 

sec 



Filter performance was investigated for Q = I, .11, and .011, 



for 



and 



P(0/-1) = Pq = E 



|^[i(0) - [!' 






(0) - X, 



= 



10 0 0 
0 10 0 
0 0 10 
0 0 0 1 



R = 



1 0 
0 1 



where the a priori )^(0/-l) is known to be a reasonably good estimate 
approximately the same accuracy as an observation. 



D. PROCESSOR PERFORMANCE; AUTHOR'S CONCLUSIONS 

Table 2 summarizes a comparison of the Kalman filter performance with 
the results of the (corrected) processing by the program presently being 
used for the cases 3. “ B. " 1» 2. “ B. " I- Figures 10, 11, 

12 and 13 are residue and error plots for the example ^ = -Olj^, R = !• 

It is seen that the Kalman filter will satisfactorily handle the data 
where the measurement noise statistics approximate those used in the 
model. However, for the noise resulting from the jitter which appears 
in the "hundreds" and "thousands" digits, the filter, as configured 
without a gate, will estimate with considerable error. The raw range 
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R2 was clean of this particular noise element, and the results as 
indicated by Figures 12 and 13 were superior to those for Rl. 

It is suggested that the Kalman filter be used as the first iteration 
processing of the Autotape output. 
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TABULATED PROCESSOR COMPARISON 
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FIGURE 10: Residue 1 vs. Time. Q = .011, R = I. 
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figure 11: Residue 2 vs. Time. 3 ^= .OH, R = I. 
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FIGURE 12: Error 1 vs. Time. Q = .011, R = I. 
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FIGURE 13: Error 2 vs. Time. Q = .011, R = I. 
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III. FUTURE FILTER IMPROVEMENTS 



The filter, as designed, will process by off-line (forward) filtering 
of the range measurements. It is suggested that, as an effort to further 
improve upon the quality of the processed data, a fixed-interval smoothing 
algorithm (the initial and final times, 0 and T, are fixed, and the 
estimate”^ (t/T) is sought) be incorporated. 

For the system and measurements described by: 

_x = Fx + 6w 
z = Hx + V 



the equations defining the forward filter are, in the time domain [Ref. 3]: 



^ = Fx + - Hx] , ^ ^ (1) 

i = fP + f L*" + - phV^hp, p(o) = p^ (2) 



dx 

To write the backward filter equations, set ^ = T - t. Then 
and 



-dx 
dt ’ 



dx 

dr 



■f^ denoting differentiation with 

respect to backward time. 



Also, 



z(r) = Hx + V. 



Then, by analogy, the backward filter equations can be written by 
changing _F to and G to -6, resulting in: 



"d^ ^ -b-- 



-ir Pb = ^ - -V-V VPb 



(3) 
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FIGURE 14: Relationship of Forward and Backward Filters 
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From Figure 14, it can be seen that the smoothed estimate at time=T 
must be the same as the forward filter estimate at that point, i.e., 

X (T/T) = t (T) 
and P (T/T) = P (T) 

which yields the required boundary condition on P^^, 

(t=T) = 0, or P^^ (r=0) = 0 (4) 

with the boundary condition on ^ (T) not yet known. Therefore, define 
the new variable: 

l(t) = (t) ^ (t) (5) 

and since ^ (T) is finite, it follows that: 



s (t=T) =0, or s (1=0) = 0. 



( 6 ) 









DA# 









1 



I 



Reformulation in terms of yields: 






Thus, equation (3) can be written as: 



V £ * £.^ i a + tf £ ' H (7) 



for which equation (4) is the appropriate boundary condition. 



Differentiating equation (5) with respect to 'T, and with some 
substitution and manipulation, we arrive at: 

i i i’' I 1 + H’' 1 (8) 




\ ‘ 

for which equation (6) is the appropriate boundary condition. Equations 
(1), (2), (7) and (8), along with: 



P"^ (t/T) = P‘* (t) + (t) 

X (t/T) = P (t/T) [p-1 (t) t (t) t P^‘ (t) % (t)] 



define the optimal smoother. 



Many forms of the smoothing equations may be derived. The form 
proposed for use in this particular case is the Rauch-Tung-Striebel form, 
with the discrete-time expressions summarized as follows: 
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Smoothed State Estimate "^(k/N) =”^(k/k) [^(k+l/N) - ^(k+l/k)] 

where 

^ = P(k/k) 0(k)"^ P(k+l/k)’^ 



for k = N-1 



Error Covariance 
Matrix Propagation 



P(k/N) = P(k/k) + ^ [P(k+1/N) - P(k+l/k)] ^ 



also for k = N-1 



Solution of the equations would proceed as follows; As an example, 
and because it is slightly easier to see when actual times are used, 
suppose NN = 100. On the forward filter pass, the values of '^(k/k), 
*^(k/k-l), P^(k/k) and Kk/k-1) would be computed and stored . On the 
final iteration of the forward pass, with K = NN = 100, 

'^(100/100) =£(100/99) + G(IOO) [z(lOO) - H ^100/99)] 
i.e., we have computed and stored ^(100/100). 

Now, the smoothing process commences in the reverse direction. 
Decrement k to k = NN-1 = 99, then 



1^(99/100) ='x(99/99) + A(99) [x(100/100) - x( 100/99)] 
stored stored stored 

and A(99) = P( 99/99) 0^ P( 100/99)’^ 

Stored stored 
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let k = NN-2 = 98, then 



^(98/100) = ^(98/98) + A(98) [ x (99/ 100 ) - x( 99/98) ] 

stored computed stored 

last 

iteration 



and A(98) = P(98/98) 0^ P(99/98)'^ 

Stored stored 

Also, for each of the two preceding iterations, 

P(99/100) = ^(99/99) + A(99) [P(100/100) - P(100/99)] A^(99) 
stored computed stored siored 

P.( 98/100) = P( 98/98) + A(98j [P(99/100) - P(99/98)] A^ (98) 
stored computed computed stored computed 



etc. 



It is seen that the smoothing process does not involve the processing 
of actual measurement data. It does, however, utilize the complete 
filtering solution, and so fixed interval smoothing cannot be done real- 
time, on-line. It must be done after all the measurement data are 
collected. Consequently, computation speed will not be the most important 
factor. Storage requirements could, however, conceivably be, in that the 
quantities to be stored on the forward pass are arrays. It is seen that, 
should an exercise run in excess of 30 minutes, retention of the data at 
each mark could require in excess of lOOK bytes of memory, which could 
limit the facilities upon which the processor could be utilized. 
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Can PROD 
zWR = H x(k/k-l) 





r 


Car 
zlWD = 
= z-H ; 


I SUB 
= z-z^R 
i(k/k-l) 



RESl = Zj^j(k) - Xj^j(k/k-l) 

RES2 = Z2^^(k) - ^(k/k-1) 
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RETURN 

END 
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END 

SUBROUTINE MWRITE(A,N,li,ND,MD) 
DIMENSION A(ND,MD> 

DO 10 I=l ,N 
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